package com.zzz.robot201.service.client;

/**
 * 与c语言通信(java做client，c/c++做server，传送一个结构)
 *
 * @author kingfish
 * @version 1.0
 */
public class ResponseMsg {
    private String hostIp;
    private Integer hostPort;
    private String deviceCode;
    private Long nowtime;

    int index = -4;
	private byte[] buf = new byte[260]; // 接收的字节数据

    public float[][] m_q_d = new float[4][3]; // 12 * 4 期望角度 [i][j]  i表示腿号，j表示关节号
    public float[][] m_q = new float[4][3];// 12 * 4   实际角度
    public float[][] m_tau_d = new float[4][3];// 12 * 4   期望力矩
    public float[][] m_tau = new float[4][3];// 12 * 4 实际力矩

   public float imu[] = new float[3]; // 3 * 4    陀螺角度
    public float battery[] = new float[1]; // 1 * 4    电池电量
    public float power_diss[] = new float[1]; // 1 * 4    功耗
    public float temperature[] = new float[1]; // 1 * 4    温度

    // 当前步态
   public short gait_id;
    // 当前步态状态
    public short gait_state_id;
    // 当前控制模式
    public short ctrl_mode;
    //机器人行进速度
    public float robot_v[] = new float[1];
    //目标点位置
    public float target_p[] = new float[2];
    //人员位置
    public float leader_p[] = new float[2];
    //网格地图标志unsigned char  *1
    public byte grid_map_flag_char = 0;
    //传感器状态unsigned char[4][3]  *12
    public byte[][] sensor_diag_char = new byte[4][3];
    //陀螺状态 unsigned char  *1
    public byte imu_state_char = 0;

    private void setFloatArray2Dimension(float[][] arrData, byte[] bArr) {
        for (int i = 0; i < arrData.length; i++) {
            for (int j = 0; j < arrData[i].length; j++) {
                index +=4;
                byte[] leg1 = new byte[4];
                System.arraycopy(bArr,index,leg1,0,leg1.length);
                arrData[i][j] = CommonBytes.getFloat(leg1);
            }
        }
    }
    private void setFloatArray1Dimension(float[] arrData, byte[] bArr) {
        for (int j = 0; j < arrData.length; j++) {
            index +=4;
            byte[] leg1 = new byte[4];
            System.arraycopy(bArr,index,leg1,0,leg1.length);
            arrData[j] = CommonBytes.getFloat(leg1);
        }
    }

    private void setUnsignedCharArray2(byte[][] sensor_state_char, byte[] bArr) {
        for (int i = 0; i < sensor_state_char.length; i++) {
            for (int j = 0; j < sensor_state_char[i].length; j++) {
                index +=1;
                byte[] leg1 = new byte[1];
                System.arraycopy(bArr,index,leg1,0,leg1.length);
                sensor_state_char[i][j] = leg1[0];//
            }
        }
    }
    private short readShortValue(short shortV, byte[] bArr) {
        /*index +=2;*/
        byte[] leg1 = new byte[2];
        System.arraycopy(bArr,index,leg1,0,leg1.length);
        //shortToByte_LH(shortV,leg1,0);
        return CommonBytes.byteToShort_HL(leg1);
    }

	public ResponseMsg(byte[] bArr) {
        setFloatArray2Dimension(m_q_d,bArr);//期望角度
        setFloatArray2Dimension(m_q,bArr);//实际角度
        setFloatArray2Dimension(m_tau_d,bArr);//期望力矩
        setFloatArray2Dimension(m_tau,bArr);//实际力矩
        setFloatArray1Dimension(imu,bArr);//陀螺角度
        setFloatArray1Dimension(battery,bArr);//电池电量
        setFloatArray1Dimension(power_diss,bArr);//功耗
        setFloatArray1Dimension(temperature,bArr);//温度

        setFloatArray1Dimension(robot_v,bArr);//机器人行进速度
        setFloatArray1Dimension(target_p,bArr);//目标点位置
        setFloatArray1Dimension(leader_p,bArr);//人员位置

        index+=4;
        gait_id = readShortValue(gait_id,bArr);//当前步态
        index+=2;
        gait_state_id = readShortValue(gait_state_id,bArr);// 当前步态状态
        index+=2;
        ctrl_mode = readShortValue(ctrl_mode,bArr);//当前控制模式

        index+=2;
        grid_map_flag_char = bArr[index];// 网格地图标志 *1
        setUnsignedCharArray2(sensor_diag_char,bArr);// 传感器状态unsigned char[4][3]  *12
        index+=1;
        imu_state_char = bArr[index];// 陀螺状态

		System.out.println(index+" client receive -> "+bArr[index]+"   > "+bArr.length);

	}

	public static ResponseMsg getResponseMsg(byte[] bArr) {
		return new ResponseMsg(bArr);
	}

    public void setHostIp(String hostIp) {
        this.hostIp = hostIp;
    }

    public String getHostIp() {
        return hostIp;
    }

    public Integer getHostPort() {
        return hostPort;
    }

    public void setHostPort(Integer hostPort) {
        this.hostPort = hostPort;
    }

    public String getDeviceCode() {
        return deviceCode;
    }

    public void setDeviceCode(String deviceCode) {
        this.deviceCode = deviceCode;
    }

    public Long getNowtime() {
        return nowtime;
    }

    public void setNowtime(Long nowtime) {
        this.nowtime = nowtime;
    }

    @Override
    public String toString() {
        return "ResponseMsg{" +
                "hostIp='" + hostIp + '\'' +
                ", hostPort=" + hostPort +
                ", deviceCode='" + deviceCode + '\'' +
                ", nowtime=" + nowtime +
                ", index=" + index +
                ", m_q_d=" + printArray(m_q_d) +
                ", m_q=" + printArray(m_q) +
                ", m_tau_d=" + printArray(m_tau_d) +
                ", m_tau=" + printArray(m_tau) +
                ", imu=" + printArray(imu) +
                ", battery=" + printArray(battery) +
                ", power_diss=" + printArray(power_diss) +
                ", temperature=" + printArray(temperature) +
                ", gait_id=" + gait_id +
                ", gait_state_id=" + gait_state_id +
                ", ctrl_mode=" + ctrl_mode +
                ", robot_v=" + printArray(robot_v) +
                ", target_p=" + printArray(target_p) +
                ", leader_p=" + printArray(leader_p) +
                ", grid_map_flag_char=" + grid_map_flag_char +
                ", sensor_diag_char=" + printArrayChar(sensor_diag_char) +
                ", imu_state_char=" + imu_state_char +
                '}';
    }

    String printArray(float[][] arr){
        StringBuilder sb = new StringBuilder("[");
        for (float[] frr:arr){
            sb.append(printArray(frr));
        }
        sb.substring(sb.length()-1,sb.length());
        sb.append("]");
        return sb.toString();
    }
    String printArray(float[] arr){
        StringBuilder sb = new StringBuilder("[");
        for (float f:arr){
            sb.append(f);
            sb.append(",");
        }
        sb = new StringBuilder(sb.substring(0,sb.length()-1));
        sb.append("]");
        return sb.toString();
    }
    String printArrayChar(byte[][] arr){
        StringBuilder sb = new StringBuilder("[");
        for (byte[] frr:arr){
            sb.append(printArrayChar(frr));
        }
        sb.substring(sb.length()-1,sb.length());
        sb.append("]");
        return sb.toString();
    }
    String printArrayChar(byte[] arr){
        StringBuilder sb = new StringBuilder("[");
        for (byte f:arr){
            sb.append(f);
            sb.append(",");
        }
        sb = new StringBuilder(sb.substring(0,sb.length()-1));
        sb.append("]");
        return sb.toString();
    }
} // end